#ifndef PCLMATCH_H
#define PCLMATCH_H

#include "params.h"
#include <Eigen/Dense>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/foreach.hpp>
#include <cmath>

class PclMatch
{
public:
    PclMatch(std::shared_ptr<Params>& params_);
    void SetParams(float min_distance, float max_distance, float min_angle, int size_threshold, int step);

    // 通用的发布点云函数
    void PublishPointCloud(const PointCloud2DPtr& cloud, ros::Publisher& pub);
    // 检查两个点云间距离大于0.01米，如果大于0.01米，且小于0.1米则连续插入点云，直到点云间距小于0.01米
    PointCloud2DPtr CheckAndInsertPoints(const PointCloud2DPtr& cloud, float min_distance, float max_distance);
    // 对点云数据进行预处理，包括下采样和去除噪声。
    // PointCloud2DPtr PreprocessPointCloud(const PointCloud2DPtr& cloud_msg, int mean_k = 50, double stddev_mul_thresh = 1.0);
    // 根据输入的点云拟合线段，并发布所有线段的点云。
    std::vector<PointCloud2DPtr> FitLines(const PointCloud2DPtr& points, ros::Publisher& line_pub, int size_threshold, float min_distance);
    // 检查线段组中是否存在直角
    PointCloud2DPtr CheckRightAngleInLines(const std::vector<PointCloud2DPtr>& lines, ros::Publisher& angle_pub, size_t step, float min_angle);
    // 回调函数，用于处理接收到的点云数据
    void PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
    void RosMsgToPointCloud2D(const sensor_msgs::PointCloud2& cloud_msg, PointCloud2D& cloud);
    void PointCloud2DToRosMsg(const PointCloud2D& cloud, sensor_msgs::PointCloud2& cloud_msg);
    bool IsRightAngle(const PointCloud2DPtr& line, size_t i, size_t step, float min_angle);
    bool isParallel(const Eigen::Vector2f& a, const Eigen::Vector2f& b, float threshold );
    float euclideanDistance(const Point2D& p1, const Point2D& p2);
    PointCloud2DPtr PreprocessPointCloud(const PointCloud2DPtr& cloud_msg, int mean_k, double stddev_mul_thresh);

private:
    ros::Subscriber sub_point_cloud;
    
    ros::Publisher pub_point_cloud;
    ros::Publisher pub_filtered;
    ros::Publisher pub_cross;
    ros::Publisher pub_keypoints;
    ros::Publisher pub_interpolated;
    ros::Publisher pub_lines;
    // 定义params
    std::shared_ptr<Params> params;

    float min_distance_;
    float max_distance_;
    float min_angle_;
    int size_threshold_;
    int step_;
};

#endif // PCLMATCH_H